#ifndef COMMANDER_H_
#define COMMANDER_H_

#include "command.h"

#include "input_state.h"
#include "rover_state.h"

#include <sys/time.h>
#include <ctime>

#include <config.h>
#include <rover-ar-cfg/servo_config.h>

#include <stdlib.h>
#include <math.h>
#include <stdio.h>

#define PI 3.14159265

struct wheel_command {
  int steering_value_0;
  int steering_value_1;
  int drive_value;
};

class Commander{
 public:
  // Constructor
  Commander(void);

  // Deconstructor
  ~Commander(void){}

  // Methods
  // Acessors
  Command* getCommand(InputState* in_state, RoverState* rvr_state);
  Command getCommand(RoverState* rvr_state);

 protected:
  double max_rotate;
  double max_mag;
  
  // Decay
  bool delaying;
  double ms_delay;
  struct timeval delay_start;

  int last_y;

  int mast_ele;
  int mast_pan;

  // 0-1 = FR, 2-3 FR, 4-5 RR, 6-7 RL
  int last_sent_wheels[8];
  double delay_start_wheels_ms[4];
  double simulated_wheel_ms[4];
  double delay_end_wheel_ms[4];

  void recordWheels(Command* output);
  void recordSimulatedWheels(Command* output);

  // DOF of arm approaching gripper;
  int last_sent_arm[12];
  void recordArm(Command* output);

  // Decay of servo steering
  void simulateSteeringDelay(void);
  void checkForWheelDelay(Command* ouput);
  void delayWheels(Command* output);
  void checkForDelayEnd(void);

  // Methods

  // Mast
  int getMast(InputState* in_state, RoverState* rvr_state, Command* output);

  // Drivetrain
  int getDriveTrain(InputState* in_state, RoverState* rvr_state, Command* output);

  // DriveTrain joystick twist
  bool enabledTwist(InputState* in_state);
  int getTwistValues(double percent, double z_percent, double omega, double throt_per, double direction, int rover_x, int rover_y, int tar_x, int tar_y, wheel_command* output);
  int getTwistWheelSpeed(double percent, double z_percent, double omega, double throt_per, double direction, int diff_x, int diff_y);
  double calculateOmega(double tar_x, double tar_y, double percent, double throt_per);

  void limitPWM(int* value, int max, int min);

  // DriveTrain straight
  int getSimpleWheelSpeed(double throt_per, int x, int y);
  int getSimpleDrive(double throt_per, int x, int y, wheel_command* output);

  // Arm
  int getArm(InputState* in_state, RoverState* rvr_state, Command* output);
  int linFit(int min_in, int max_in, int min_out, int max_out, int value);

  // Conversions
  bool notInDeadBand(int value, int center, int deadband);
  int radsToMS(double rads);
  void splitToTwo(int input, int* value_0, int* value_1);

  // Vertify
  bool validWheelCommand(wheel_command* input);

  // Bootup methods
  void computeMaxRotation(void);
  void computeMaxMag(void);
};



#endif
